// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ArmPositionDisplayAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Manipulation.Visualization
{
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.Collections.ObjectModel;
    using System.IO;
    using System.Runtime.Serialization;
    using System.Text;
    using System.Xml;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Manipulation.Runtime.Arm;
    using Microsoft.Robotics.Manipulation.Runtime.MotionControl;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Visualization;

    /// <summary>
    /// This model agent class prepares visualization for arm manipulation.
    /// </summary>
    [DataContract]
    public sealed class ArmPositionDisplayAgent : ModelAgent<ManipulatorStatusMessage, ArmPositionDisplayModel>
    {
        /// <summary>
        /// Builds display representation of the data
        /// </summary>
        private StringBuilder arrayDisplay;

        /// <summary>
        /// Name of the manipulation agent
        /// </summary>
        [DataMember(Name = "ManipulationAgentName")]
        private string manipulationAgentName = default(string);

        /// <summary>
        /// Name of the manipulation agent
        /// </summary>
        [DataMember(Name = "SplineAgentName")]
        private string splineAgentName = default(string);

        /// <summary>
        /// Maximum cartesian move error tolerance
        /// </summary>
        [DataMember(Name = "CartesianMoveTolerance")]
        private double cartesianMoveTolerance;

        /// <summary>
        /// Maximum joint move error tolerance
        /// </summary>
        [DataMember(Name = "JointMoveTolerance")]
        private double jointMoveTolerance;

        /// <summary>
        /// Control port for spline control
        /// </summary>
        private IForwarder<StartTrajectoryMessage> armSplineControlPort;

        /// <summary>
        /// Control port for arm cartesian control
        /// </summary>
        private IForwarder<CartesianGoalPositionControlMessage> armCartesianControlPort;

        /// <summary>
        /// Control port for arm cartesian touch control
        /// </summary>
        private IForwarder<LinearCartesianTouchControlMessage> armCartesianTouchControlPort;

        /// <summary>
        /// Control port for arm Cartesian push control
        /// </summary>
        private IForwarder<LinearCartesianPushControlMessage> armCartesianPushControlPort;

        /// <summary>
        /// Control port for moving to home position
        /// </summary>
        private IForwarder<MoveToHomePositionMessage> armMoveToHomePositionPort;

        /// <summary>
        /// Control port for arm cartesian control
        /// </summary>
        private IForwarder<JointGoalPositionControlMessage> armJointControlPort;

        /// <summary>
        /// Initializes a new instance of the ArmPositionDisplayAgent class
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="producer">The name of the agent we subscribe to.</param>
        /// <param name="maxUpdateIntervalMilliseconds">The interval on which to issue data binding updates to the UI</param>
        /// <param name="manipulationAgentName">Manipulation agent name</param>
        /// <param name="splineAgentName">Spline agent name.</param>
        /// <param name="cartesianControlTolerance">Maximum cartesian move error tolerance</param>
        /// <param name="jointControlTolerance">Maximum joint move error tolerance</param>
        public ArmPositionDisplayAgent(
            string name, 
            IProducer<ManipulatorStatusMessage> producer,
            int maxUpdateIntervalMilliseconds,
            string manipulationAgentName,
            string splineAgentName,
            double cartesianControlTolerance,
            double jointControlTolerance)
            : base(name, producer.Name, maxUpdateIntervalMilliseconds)
        {
            this.manipulationAgentName = manipulationAgentName;
            this.cartesianMoveTolerance = cartesianControlTolerance;
            this.jointMoveTolerance = jointControlTolerance;
            this.splineAgentName = splineAgentName;
        }

        /// <summary>
        /// Initializes any fields not provided in the manifest.
        /// </summary>
        /// <param name="locator">Not used.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.arrayDisplay = new StringBuilder();

            if (this.cartesianMoveTolerance <= 0.0)
            {
                throw new ArgumentException("cartesianMoveTolerance must be > 0. Got " + this.cartesianMoveTolerance.ToString());
            }

            if (this.jointMoveTolerance <= 0.0)
            {
                throw new ArgumentException("jointMoveTolerance must be > 0. Got " + this.jointMoveTolerance.ToString());
            }

            IAgentHost manipulatorAgent = this.ProducerAgent;

            manipulatorAgent.Subscribe<ManipulatorStatusMessage>(this);

            try
            {
                this.armJointControlPort = manipulatorAgent.GetControlPort<JointGoalPositionControlMessage>();
            }
            catch (InvalidOperationException)
            {
                this.armJointControlPort = null;
            }

            try
            {
            this.armCartesianTouchControlPort = manipulatorAgent.GetControlPort<LinearCartesianTouchControlMessage>();
            }
            catch (InvalidOperationException)
            {
                this.armCartesianTouchControlPort = null;
            }

            try
            {
            this.armCartesianPushControlPort = manipulatorAgent.GetControlPort<LinearCartesianPushControlMessage>();
            }
            catch (InvalidOperationException)
            {
                this.armCartesianPushControlPort = null;
            }

            try
            {
                this.armMoveToHomePositionPort = manipulatorAgent.GetControlPort<MoveToHomePositionMessage>();
            }
            catch (InvalidOperationException)
            {
                this.armMoveToHomePositionPort = null;
            }

            if (false == string.IsNullOrEmpty(this.manipulationAgentName))
            {
                manipulatorAgent = locator.GetAgent(this.manipulationAgentName);
            }

            this.armCartesianControlPort = manipulatorAgent.GetControlPort<CartesianGoalPositionControlMessage>();

            if (!string.IsNullOrEmpty(this.splineAgentName))
            {
                IAgentHost splineAgent = locator.GetAgent(this.splineAgentName);
                this.armSplineControlPort = splineAgent.GetControlPort<StartTrajectoryMessage>();
            }
            else
            {
                this.armSplineControlPort = null;
            }

            this.Model.ExtendedSplinePoints = new ObservableCollection<Tuple<string, double>>();
        }

        /// <summary>
        /// Updates the model data with the new state.
        /// </summary>
        /// <param name="message">The new state from the upstream agent.</param>
        /// <returns>true if the model was updated, false otherwise</returns>
        protected override bool UpdateModel(ManipulatorStatusMessage message)
        {
            ManipulatorStatus state = message.State;

            this.MakeStringFromVector3(this.arrayDisplay, message.State.TcpCartesianPosition.Position);
            this.Model.CurrentCartesianPosition = this.arrayDisplay.ToString();

            this.MakeStringFromQuaternion(this.arrayDisplay, message.State.TcpCartesianPosition.Orientation);
            this.Model.CurrentCartesianOrientation = this.arrayDisplay.ToString();

            this.MakeStringFromVector3(this.arrayDisplay, message.State.TcpCommandedCartesianPosition.Position);
            this.Model.TargetCartesianPosition = this.arrayDisplay.ToString();

            this.MakeStringFromQuaternion(this.arrayDisplay, message.State.TcpCommandedCartesianPosition.Orientation);
            this.Model.TargetCartesianOrientation = this.arrayDisplay.ToString();

            AxisAngle curAxis = Quaternion.ToAxisAngle(Quaternion.Normalize(message.State.TcpCartesianPosition.Orientation));
            this.MakeStringFromVector3(this.arrayDisplay, curAxis.Axis);
            double angdeg = curAxis.Angle * 180 / Math.PI;
            this.Model.CurrentAxisAngle = string.Format("{0} {1:F1}", this.arrayDisplay.ToString(), angdeg);

            if (this.Model.DoUpdatePosition)
            {
                string name = this.Model.CurrentCartesianPositionName;

                if (this.Model.CartesianPositions.ContainsKey(name))
                {
                    this.Model.CartesianPositions[name] = message.State.TcpCartesianPosition;
                }
                else
                {
                    this.Model.CartesianPositions.Add(
                        name,
                        message.State.TcpCartesianPosition);

                    ObservableCollection<string> newStringCollection =
                        new ObservableCollection<string>(this.Model.CartesianPositionNames);

                    newStringCollection.Add(name);
                    this.Model.CartesianPositionNames = newStringCollection;
                }

                this.Model.DoUpdatePosition = false;
            }

            if (this.Model.DoGoToHome == true && this.armMoveToHomePositionPort != null)
            {
                this.Model.DoGoToHome = false;
                if (this.Model.GoToHomeSpeed > 0 && this.Model.GoToHomeSpeed < 1)
                {
                    this.armMoveToHomePositionPort.Post(
                        new MoveToHomePositionMessage(
                            message.OriginatingTime,
                            this.Model.GoToHomeSpeed));
                }
            }

            // Send a cartesian position
            if (this.Model.DoGoToPosition == true && null != this.armCartesianControlPort)
            {
                Pose target = this.Model.CartesianPositions[this.Model.CurrentlySelectedCartesianPositionName];

                this.Model.CurrentCartesianPositionName = this.Model.CurrentlySelectedCartesianPositionName;

                CartesianGoalPositionControlMessage msg = new CartesianGoalPositionControlMessage(
                    message.OriginatingTime, 
                    target,
                    this.cartesianMoveTolerance);

                this.armCartesianControlPort.Post(msg);

                this.Model.DoGoToPosition = false;
            }
            else if (this.Model.DoCartesianManualUpdate == true && null != this.armCartesianControlPort)
            {
                double[] posArray = this.MakeArrayFromString(this.Model.CartesianPositionManual);
                double[] orientArray = this.MakeArrayFromString(this.Model.CartesianOrientationManual);

                if (posArray.Length == 3 && orientArray.Length == 4)
                {
                    Pose target = new Pose(
                            new Vector3(posArray[0], posArray[1], posArray[2]),
                            new Quaternion(orientArray[1], orientArray[2], orientArray[3], orientArray[0]));

                    CartesianGoalPositionControlMessage msg = new CartesianGoalPositionControlMessage(
                    message.OriginatingTime,
                    target,
                    this.cartesianMoveTolerance);

                    this.armCartesianControlPort.Post(msg);
                }

                this.Model.DoCartesianManualUpdate = false;
            }

            if (this.Model.DoTouchPosition == true && null != this.armCartesianTouchControlPort)
            {
                this.Model.DoTouchPosition = false;

                double forceThreshold = Convert.ToDouble(this.Model.TouchForceThresholdText);
                double maxLinSpeed = Convert.ToDouble(this.Model.TouchMaxLinSpeedText);
                double maxRotSpeed = Convert.ToDouble(this.Model.TouchMaxRotSpeedText);

                Pose target = this.Model.CartesianPositions[this.Model.CurrentlySelectedCartesianPositionName];

                LinearCartesianTouchControlMessage msg = new LinearCartesianTouchControlMessage(
                    message.OriginatingTime,
                    target,
                    maxLinSpeed,
                    maxRotSpeed,
                    forceThreshold);

                    this.armCartesianTouchControlPort.Post(msg);
            }

            if (this.Model.DoPushPosition && this.armCartesianPushControlPort != null)
            {
                this.Model.DoPushPosition = false;

                double pushForceThreshold = Convert.ToDouble(this.Model.PushAxisForceThresholdText);
                double residualForceThreshold = Convert.ToDouble(this.Model.PushResidualAxisForceThresholdText);
                double maxLinSpeed = Convert.ToDouble(this.Model.PushMaxLinSpeedText);
                double maxRotSpeed = Convert.ToDouble(this.Model.PushMaxRotSpeedText);

                Vector3 pushAxis = new Vector3(
                    Convert.ToDouble(this.Model.PushAxisXText),
                    Convert.ToDouble(this.Model.PushAxisYText),
                    Convert.ToDouble(this.Model.PushAxisZText));

                Pose target = this.Model.CartesianPositions[this.Model.CurrentlySelectedCartesianPositionName];

                LinearCartesianPushControlMessage msg = new LinearCartesianPushControlMessage(
                    message.OriginatingTime,
                    target,
                    maxLinSpeed,
                    maxRotSpeed,
                    pushAxis,
                    pushForceThreshold,
                    residualForceThreshold);

                this.armCartesianPushControlPort.Post(msg);
            }

            if (this.Model.SaveCartesianPositions)
            {
                this.Model.SaveCartesianPositions = false;

                // Configure save file dialog box
                Microsoft.Win32.SaveFileDialog dlg = new Microsoft.Win32.SaveFileDialog();
                dlg.FileName = "Positions"; 
                dlg.DefaultExt = ".xml"; 
                dlg.Filter = "XML documents (.xml)|*.xml"; // Filter files by extension 

                // Show save file dialog box
                bool? result = dlg.ShowDialog();

                // Process save file dialog box results 
                if (result == true)
                {
                    // Save document 
                    string filename = dlg.FileName;

                    using (FileStream writer = new FileStream(filename, FileMode.Create))
                    {
                        DataContractSerializer ser = new DataContractSerializer(
                            typeof(Dictionary<string, Pose>));
                        ser.WriteObject(writer, this.Model.CartesianPositions);
                    }
                }
            }

            if (this.Model.LoadCartesianPositions)
            {
                this.Model.LoadCartesianPositions = false;

                // Configure open file dialog box
                Microsoft.Win32.OpenFileDialog dlg = new Microsoft.Win32.OpenFileDialog();
                dlg.FileName = "Positions"; // Default file name
                dlg.DefaultExt = ".xml"; // Default file extension
                dlg.Filter = "XML documents (.xml)|*.xml"; // Filter files by extension 

                // Show open file dialog box
               bool? result = dlg.ShowDialog();

                // Process open file dialog box results 
                if (result == true)
            {
                    // Open document 
                    string filename = dlg.FileName;

                    using (FileStream fs = new FileStream(filename, FileMode.Open))
                    {
                        XmlDictionaryReader reader =
                            XmlDictionaryReader.CreateTextReader(fs, new XmlDictionaryReaderQuotas());

                        DataContractSerializer ser = new DataContractSerializer(typeof(Dictionary<string, Pose>));

                        this.Model.CartesianPositions = (Dictionary<string, Pose>)ser.ReadObject(reader, true);
                    }

                    this.Model.CartesianPositionNames = new ObservableCollection<string>();
                    foreach (string s in this.Model.CartesianPositions.Keys)
                    {
                        this.Model.CartesianPositionNames.Add(s);
                    }
                }
            }

            if (this.Model.DoAddUpdateJointPosition)
            {
                string name = this.Model.CurrentJointPositionName;

                double[] jointPositions = new double[message.State.JointPositions.Length];
                Array.Copy(message.State.JointPositions, jointPositions, message.State.JointPositions.Length);

                if (this.Model.JointPositions.ContainsKey(name))
                {
                    this.Model.JointPositions[name] = jointPositions;
                }
                else
                {
                    this.Model.JointPositions.Add(
                        name,
                        jointPositions);

                    ObservableCollection<string> newStringCollection =
                        new ObservableCollection<string>(this.Model.JointPositionNames);

                    newStringCollection.Add(name);
                    this.Model.JointPositionNames = newStringCollection;
                }

                this.Model.DoAddUpdateJointPosition = false;
            }

            if (this.Model.SaveJointPositions)
            {
                this.Model.SaveJointPositions = false;

                // Configure save file dialog box
                Microsoft.Win32.SaveFileDialog dlg = new Microsoft.Win32.SaveFileDialog();
                dlg.FileName = "Positions"; 
                dlg.DefaultExt = ".xml"; 
                dlg.Filter = "XML documents (.xml)|*.xml"; // Filter files by extension 

                // Show save file dialog box
                bool? result = dlg.ShowDialog();

                // Process save file dialog box results 
                if (result == true)
                {
                    // Save document 
                    string filename = dlg.FileName;

                    using (FileStream writer = new FileStream(filename, FileMode.Create))
                    {
                        DataContractSerializer ser = new DataContractSerializer(
                            typeof(Dictionary<string, double[]>));
                        ser.WriteObject(writer, this.Model.JointPositions);
                    }
                }
            }

            if (this.Model.LoadJointPositions)
            {
                this.Model.LoadJointPositions = false;

                // Configure open file dialog box
                Microsoft.Win32.OpenFileDialog dlg = new Microsoft.Win32.OpenFileDialog();
                dlg.FileName = "Positions"; // Default file name
                dlg.DefaultExt = ".xml"; // Default file extension
                dlg.Filter = "XML documents (.xml)|*.xml"; // Filter files by extension 

                // Show open file dialog box
               bool? result = dlg.ShowDialog();

                // Process open file dialog box results 
                if (result == true)
            {
                    // Open document 
                    string filename = dlg.FileName;

                    using (FileStream fs = new FileStream(filename, FileMode.Open))
                    {
                        XmlDictionaryReader reader =
                            XmlDictionaryReader.CreateTextReader(fs, new XmlDictionaryReaderQuotas());

                        DataContractSerializer ser = new DataContractSerializer(typeof(Dictionary<string, double[]>));

                        this.Model.JointPositions = (Dictionary<string, double[]>)ser.ReadObject(reader, true);
                    }

                    this.Model.JointPositionNames = new ObservableCollection<string>();
                    foreach (string s in this.Model.JointPositions.Keys)
                    {
                        this.Model.JointPositionNames.Add(s);
                    }
                }
            }

            // Send a joint position
            if (this.Model.DoGoToJointPosition == true && null != this.armJointControlPort)
            {
                double[] target = this.Model.JointPositions[this.Model.CurrentlySelectedJointPositionName];

                this.Model.CurrentJointPositionName = this.Model.CurrentlySelectedJointPositionName;

                JointGoalPositionControlMessage msg = new JointGoalPositionControlMessage(
                    message.OriginatingTime,
                    target,
                    0);

                this.armJointControlPort.Post(msg);

                this.Model.DoGoToJointPosition = false;
            }

            if (this.Model.DoGoToSplineJointPosition
                && this.armSplineControlPort != null
                && !string.IsNullOrEmpty(this.Model.CurrentlySelectedJointPositionName))
            {
                CubicSpline spline = new CubicSpline(
                    message.State.JointPositions.Length,
                    2);

                for (int i = 0; i < spline.NumDOF; ++i)
                {
                    spline.TargetPositions[i, 1] = this.Model.JointPositions[this.Model.CurrentlySelectedJointPositionName][i];
                }

                spline.Times[1] = 20;

                StartTrajectoryMessage msg = new StartTrajectoryMessage(
                    message.OriginatingTime,
                    spline.NumDOF,
                    spline.NumPoints,
                    spline.Times,
                    spline.TargetPositions,
                    spline.TargetVelocities,
                    TrajectoryInterpolationMethods.CubicSpline,
                    true);

                this.armSplineControlPort.Post(msg);

                this.Model.DoGoToSplineJointPosition = false;
            }

            if (this.Model.DoExecuteExtendedSpline
                && null != this.armSplineControlPort
                && this.Model.ExtendedSplinePoints.Count > 0)
            {
                // if the first spline point has a non-zero time, then
                // we will use the current position as the first point on the spline
                bool useCurrentAsStartPosition = false;
                int numPoints = this.Model.ExtendedSplinePoints.Count;

                if (this.Model.ExtendedSplinePoints[0].Item2 != 0)
                {
                    useCurrentAsStartPosition = true;
                    ++numPoints;
                }

                CubicSpline spline = new CubicSpline(
                    message.State.JointPositions.Length,
                    numPoints);

                int pointOffset = 0;
                if (!useCurrentAsStartPosition)
                {
                    spline.Times[0] = this.Model.ExtendedSplinePoints[0].Item2;
                    for (int i = 0; i < spline.NumDOF; ++i)
                    {
                        spline.TargetPositions[i, 0] = this.Model.JointPositions[this.Model.ExtendedSplinePoints[0].Item1][i];
                    }
                }
                else
                {
                    // in this case all of the parameters for the first spline point should be 0 (defaults),
                    // so no need to set target positions or time here.
                    ++pointOffset;
                }

                for (int i = 1; i < this.Model.ExtendedSplinePoints.Count + pointOffset; ++i)
                {
                    for (int dof = 0; dof < spline.TargetPositions.GetLength(0); ++dof)
                    {
                        spline.TargetPositions[dof, i] = this.Model.JointPositions[this.Model.ExtendedSplinePoints[i - pointOffset].Item1][dof];
                        spline.Times[i] = spline.Times[i - 1] + this.Model.ExtendedSplinePoints[i - pointOffset].Item2;
                    }
                }

                StartTrajectoryMessage msg = new StartTrajectoryMessage(
                    message.OriginatingTime,
                    spline.NumDOF,
                    spline.NumPoints,
                    spline.Times,
                    spline.TargetPositions,
                    spline.TargetVelocities,
                    TrajectoryInterpolationMethods.CubicSpline,
                    useCurrentAsStartPosition,
                    true,
                    true);

                this.armSplineControlPort.Post(msg);

                this.Model.DoExecuteExtendedSpline = false;
            } 

            // Joint positions
            this.MakeStringFromArray(this.arrayDisplay, state.JointPositions);
            this.Model.CurrentJointPosition = this.arrayDisplay.ToString();

            this.MakeStringFromArray(this.arrayDisplay, state.CommandedJointPositions);
            this.Model.TargetJointPosition = this.arrayDisplay.ToString();

            this.MakeStringFromArray(this.arrayDisplay, state.JointVelocities);
            this.Model.CurrentJointVelocity = this.arrayDisplay.ToString();

            double[] diff = new double[state.JointPositions.Length];
            for (int i = 0; i < state.JointPositions.Length; ++i)
            {
                diff[i] = state.JointPositions[i] - state.CommandedJointPositions[i];
            }

            this.MakeStringFromArray(this.arrayDisplay, diff);
            this.Model.DeltaJointPosition = this.arrayDisplay.ToString();

            if (this.Model.DoJointUpdate == true && null != this.armJointControlPort)
            {
                double[] jointMove = this.MakeArrayFromString(this.Model.JointManualEntry);
                if (jointMove.Length == state.CommandedJointPositions.Length)
                {
                    JointGoalPositionControlMessage msg = new JointGoalPositionControlMessage(message.OriginatingTime, jointMove, this.jointMoveTolerance);
                    this.armJointControlPort.Post(msg);
                }

                this.Model.DoJointUpdate = false;
            }

            return true;
        }

        /// <summary>
        /// Builds a string from an array by formatting a string containing all values
        /// </summary>
        /// <param name="sb">Place to build string</param>
        /// <param name="array">array values</param>
        private void MakeStringFromArray(StringBuilder sb, double[] array)
        {
            sb.Clear();
            int i = 0;
            sb.AppendFormat("{0:G3}", array[i]);

            for (++i; i < array.Length; ++i)
            {
                sb.AppendFormat("\t{0:G3}", array[i]);
            }
        }

        /// <summary>
        /// Builds a string from an vector3 by formatting a string containing all values
        /// </summary>
        /// <param name="sb">Place to build string</param>
        /// <param name="vector">vector3 values</param>
        private void MakeStringFromVector3(StringBuilder sb, Vector3 vector)
        {
            sb.Clear();
            sb.AppendFormat("{0:G4} {1:G4} {2:G4}", vector.X, vector.Y, vector.Z);
        }

        /// <summary>
        /// Builds a string from an quaternion by formatting a string containing all values
        /// </summary>
        /// <param name="sb">Place to build string</param>
        /// <param name="quaternion">quaternion values</param>
        private void MakeStringFromQuaternion(StringBuilder sb, Quaternion quaternion)
        {
            sb.Clear();
            sb.AppendFormat("{0:G4} {1:G4} {2:G4} {3:G4}", quaternion.W, quaternion.X, quaternion.Y, quaternion.Z);
        }

        /// <summary>
        /// Tokenize string on white space and return it as an array of doubles
        /// </summary>
        /// <param name="text">Input string</param>
        /// <returns>Array of doubles</returns>
        private double[] MakeArrayFromString(string text)
        {
            text = text.Replace(',', ' ');
            string[] fields = text.Split(" \t".ToCharArray(), StringSplitOptions.RemoveEmptyEntries);
            double[] ret = new double[fields.Length];

            for (int i = 0; i < fields.Length; ++i)
            {
                double d;
                if (false == double.TryParse(fields[i], out d))
                {
                    d = 0.0;
                }

                ret[i] = d;
            }

            return ret;
        }

        /// <summary>
        /// Get a pose as a 6 dimensional array of position and Euler angles.
        /// </summary>
        /// <param name="pose">A pose.</param>
        /// <returns>A 6 dimensional array of position and Euler angles.</returns>
        private double[] MakePositionPlusEulerAngleArrayFromPose(Pose pose)
        {
            double[] poseArray = new double[6];

            Vector3 eulerAngles = pose.GetZYXEulerAngles();

            poseArray[0] = pose.Position.X;
            poseArray[1] = pose.Position.Y;
            poseArray[2] = pose.Position.Z;

            poseArray[3] = eulerAngles.X;
            poseArray[4] = eulerAngles.Y;
            poseArray[5] = eulerAngles.Z;

            return poseArray;
        }
    }
}
